#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <stdint.h>
#include <pthread.h>
#include <thread>

#include "ArmorPlate.h"     //非系统库
#include "camera_class.h"   //非系统库
#include "Serial.h"      //非系统库
#include "solveangle.hpp"

using namespace cv;
using namespace std;

extern float S_yaw;
extern float S_pitch;

// extern float R_yaw;
// extern float R_pitch;

// extern bool pre_mode = false;   //是否开启预测模式

int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop);
void *send_thread(void *arg);
void *receive_thread(void *arg);
void *monitor_thread(void *arg);
void CameraLoop();

int fd; // 全局文件描述符，供线程共享
volatile int keep_running = 1; // 控制线程退出的变量

// 新的结构体定义
#pragma pack(1)
typedef struct {
     float yaw;
     float pitch;
     uint8_t fire;   // 0: 不开火, 1: 开火
     int16_t distance;
} VisionSendStructTypedef;
#pragma pack()

#pragma pack(1)
typedef struct {
    float yaw;
    float pitch;
} VisionRecvStructTypedef;
#pragma pack()

int main()
{
    // Serial serial;
    // serial.autoSerial();
    char *uart0 = (char *)"/dev/ttyUSB0";
    pthread_t send_tid, recv_tid, monitor_tid;

    fd = open(uart0, O_RDWR | O_NOCTTY | O_NDELAY);
    if (-1 == fd)
    {
        perror("无法打开串口");
        return -1;
    }
    else
    {
        printf("成功打开串口。\n");
        if (-1 == set_opt(fd, 115200, 8, 'N', 1))
        {
            perror("配置串口失败");
            close(fd);
            return -1;
        }
    }


    // 设置为阻塞模式
    fcntl(fd, F_SETFL, 0);

    thread t1(CameraLoop);

    // // 创建发送线程
    // if (0 != pthread_create(&send_tid, NULL, send_thread, NULL))
    // {
    //     perror("创建发送线程失败");
    //     close(fd);
    //     return -1;
    // }

    // // 创建接收线程
    // if (0 != pthread_create(&recv_tid, NULL, receive_thread, NULL))
    // {
    //     perror("创建接收线程失败");
    //     close(fd);
    //     return -1;
    // }

    // if(0 != pthread_create(&monitor_tid, NULL, monitor_thread, NULL))
    // {
    //     perror("创建接收线程失败");
    //     close(fd);
    //     return -1;
    // }

    // 等待线程结束
    // pthread_join(send_tid, NULL);
    //pthread_join(recv_tid, NULL);
    // pthread_join(monitor_tid, NULL);
    t1.join();

    while('q' != getchar())
        continue;
    // 退出线程
    keep_running = 0;

    close(fd);

    return 0;
}

void CameraLoop()      //海康相机获取画面线程
{
    camera cam;
    Mat image;
    cam.start_cam();    //开启摄像头

    while (true)
    {
        cam.get_pic(&image);  //获取画面
        flip(image, image, 1);  //图像翻转
        if(image.empty())
        {
            cout << "Find no image!" << endl;
            break;
        }
        AngleSolve foo(image);
        // armor.autoArmorPlate();     //调用找装甲板模块
        S_yaw = -float(foo.yaw_get());
        S_pitch = float(foo.pitch_get());
        Mat img = foo.img_get();
        imshow("result", img);
        if('q' == waitKey(1))
        {
            cam.close_cam();
            break;
        }
    }
}

uint8_t a = 0;

void *send_thread(void *arg)
{
    //static int a = 0;

    VisionSendStructTypedef message = {S_yaw, S_pitch, 0x02, 0x99}; // 示例数据
    unsigned char bufferW[sizeof(message) + 3];
    while (keep_running)
    {
        bufferW[0] = 0xFF;                                              // 标识符
        message.yaw=S_yaw;
        message.pitch=S_pitch;
        S_yaw=0;
        S_pitch=0;
        bufferW[1] = sizeof(message);                                               
        cout << "yaw:  " << S_yaw << "  pitch:  " << S_pitch << endl;   
        memcpy(bufferW + 2, &message, sizeof(message));                 // 拷贝结构体数据  
        bufferW[sizeof(message) + 2] = 0xFE;           
        int n = write(fd, bufferW, sizeof(message) + 3);
        if (n < 0)
        {
            perror("发送数据失败");
            break;
        }
        else
        {
            printf("发送: Yaw = %f, Pitch = %f, Fire = %d, Distance = %d, end = 0x%0X\n", message.yaw, message.pitch, message.fire, message.distance, bufferW[sizeof(message)+2]);
            fflush(stdout);
        }
        usleep(100000); // 延时
    }
    exit(-1);
    return NULL;
}

typedef enum
{
    eReNone = 0,
    eReHead,
    eReLen,
    eReData,
    eReTail,
} ERecvStatusTypedef;
#define HEAD_FRAME (0xFF)
#define TAIL_FRAME (0xFE)

VisionRecvStructTypedef g_sRecvData;

void vision_recdata_in(uint8_t data)
{
    static unsigned char buffer[sizeof(g_sRecvData)];
    static uint8_t _cvSta = 0;
    static unsigned char _opt;
    static uint8_t _lens;

    switch(_cvSta)
    {
        case eReNone:
            if(data == HEAD_FRAME)
            {
                _cvSta = eReHead;
                memset(&buffer[0], 0, sizeof(buffer));
                _opt = 0;
            }
            else
            {
                _cvSta = eReNone;
                _opt = 0;
            }
            break;
        case eReHead:
            _lens = data;
            _cvSta = eReLen;
            _opt = 0;
            break;
        case eReLen:
        
            // _opt++;
            buffer[_opt++] = data;
            fflush(stdout);
            if(_opt >= _lens)
            {
                _cvSta = eReData;
            }
            break;
        case eReData:
            if(data == TAIL_FRAME)
            {
                fflush(stdout);
                memcpy(&g_sRecvData, &buffer[0], sizeof(buffer));
                // printf("yaw = %f, pitch = %f\n", g_sRecvData.yaw, g_sRecvData.pitch);
                // // R_yaw = g_sRecvData.yaw;
                // R_pitch = g_sRecvData.pitch;
                _cvSta = eReNone;
            }
            else
            {
                _cvSta = eReNone;
                
            }
            break;
        case eReTail:
            _cvSta = eReNone;
            _opt = 0;
            break;
        default:
            break;
    }
}

void* receive_thread(void *arg)
{
    uint8_t buffer_rec[2];

    while(keep_running)
    {
        // printf("ok");
        static int nByte;
        nByte = read(fd, buffer_rec, 1);
        if(nByte==1)
        {
            vision_recdata_in(buffer_rec[0]);
            // tcflush(fd,TCIOFLUSH);
            nByte = 0;
        }  
    }
    exit(-1);
    return nullptr;
}

void* monitor_thread(void *arg)
{
    printf("按q退出程序...\n");
    while(keep_running)
    {
        printf("检测执行中\n");
        fflush(stdout);
        char c = getchar();
        if('q' == c || c == 'Q')
        {
            keep_running = 0;
            int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
            fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);
            printf("程序即将退出...\n");
            break;
        }
        usleep(100000);
    }
    exit(-1);
    return nullptr;
}

int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (0 != tcgetattr(fd, &oldtio))
    {
        perror("无法获取串口属性");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));

    newtio.c_cflag |= CLOCAL | CREAD;

    newtio.c_cflag &= ~CSIZE;
    switch (nBits)
    {
        case 7:
            newtio.c_cflag |= CS7;
            break;
        case 8:
        default:
            newtio.c_cflag |= CS8;
            break;
    }

    switch (nEvent)
    {
        case 'O':
            newtio.c_cflag |= PARENB | PARODD;
            newtio.c_iflag |= INPCK;
            break;
        case 'E':
            newtio.c_cflag |= PARENB;
            newtio.c_cflag &= ~PARODD;
            newtio.c_iflag |= INPCK;
            break;
        case 'N':
        default:
            newtio.c_cflag &= ~PARENB;
            break;
    }

    switch (nSpeed)
    {
        case 2400:
            cfsetispeed(&newtio, B2400);
            cfsetospeed(&newtio, B2400);
            break;
        case 4800:
            cfsetispeed(&newtio, B4800);
            cfsetospeed(&newtio, B4800);
            break;
        case 9600:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        case 115200:
        default:
            cfsetispeed(&newtio, B115200);
            cfsetospeed(&newtio, B115200);
            break;
    }

    if (2 == nStop)
        newtio.c_cflag |= CSTOPB;
    else
        newtio.c_cflag &= ~CSTOPB;

    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 1;

    tcflush(fd, TCIFLUSH);
    if (0 != (tcsetattr(fd, TCSANOW, &newtio)))
    {
        perror("设置串口属性失败");
        return -1;
    }
    return 0;
}
